-- Paket fÃ¶r att rotera en vektor
-- Riktningen pÃ¥ rotationen sker efter hÃ¶gerhandsregeln (http://sv.wikipedia.org/wiki/HÃ¶gerhandsregeln)
-- Paketet finns fÃ¶r att rotera positionen pÃ¥ "legobitarna"
-- Rotationen kan utfÃ¶ras runt valfri axel (Rotation axis) som anges i 'X', 'Y' eller 'Z'
-- Rotationen utfÃ¶rs med vinkeln Rotation_Vector som anges i radianer
-- FÃ¶r att rotera en vektor kalla pÃ¥ funktionen eller proceduren "Rotate_Single_Point_Around_Axis"

-- BÃ¥de funktionen och proceduren "Rotate_Single_Point_Around_Axis" Ã¤r skapad

-- FÃ¶r att rotera en hel sammanhÃ¤ngande bit/block mÃ¥ste funktionen kallas pÃ¥ flera ggr eller sÃ¥ bÃ¶r en ny funktion/procedur skrivas i detta paket

-- typvariabeln Float_Vector Ã¤r public som anvÃ¤nds fÃ¶r att ange en koordinat (X,Y,Z) (kanske inte Ã¤r optimalt?)

-- Allt rÃ¤knas i Float fÃ¶r att det ska fungera med alla vinklar

with Ada.Numerics.Elementary_Functions;
use Ada.Numerics.Elementary_Functions;
with Ada.Text_IO; use Ada.Text_IO;
with Ada.Integer_Text_IO; use Ada.Integer_Text_IO;

package body Rotation_Package is

procedure Matrix_Put(M : in Int_Matrix) is -- Printa Matris (anvÃ¤nds inte)
begin
   for I in 1..3 loop
     for J in 1..3 loop
        Ada.Integer_Text_IO.Put(M(J,I));
        Put(' ');
     end loop;
     New_Line;
   end loop;
end Matrix_Put;

procedure Vector_Put(V : in Int_Vector) is -- Printa vektor
begin
   for I in 1..3 loop
      Ada.Integer_Text_IO.Put(V(I),0);
      Put(' ');
      New_Line;
   end loop;
end Vector_Put;

-- Funktion fÃ¶r att skapa en rotations-matris
-- Den anvÃ¤nds sen i funktionen "Rotate_Single_Point_Around_Axis"

function Get_Rotation_Matrix(Rotation_Angle : Float;
                             Rotation_Axis : Character) return Int_Matrix is
   Rotation_Matrix : Int_Matrix;
begin
   for I in 1..3 loop -- Borde kÃ¶re "others" istÃ¤llet
      for J in 1..3 loop
         Rotation_Matrix(J,I) := 0;
      end loop;
   end loop;
   if Rotation_Axis = 'X' then
      Rotation_Matrix(1,1) := 1;
      Rotation_Matrix(2,2) := Integer(Cos(Rotation_Angle));
      Rotation_Matrix(2,3) := Integer(-sin(Rotation_Angle));
      Rotation_Matrix(3,3) := Integer(Cos(Rotation_Angle));
      Rotation_Matrix(3,2) := Integer(Sin(Rotation_Angle));
   elsif Rotation_Axis = 'Y' then
      Rotation_Matrix(1,1) := Integer(Cos(Rotation_Angle));
      Rotation_Matrix(1,3) := Integer(Sin(Rotation_Angle));
      Rotation_Matrix(2,2) := 1;
      Rotation_Matrix(3,3) := Integer(Cos(Rotation_Angle));
      Rotation_Matrix(3,1) := Integer(-Sin(Rotation_Angle));
   elsif Rotation_Axis = 'Z' then
      Rotation_Matrix(1,1) := Integer(Cos(Rotation_Angle));
      Rotation_Matrix(1,2) := Integer(-Sin(Rotation_Angle));
      Rotation_Matrix(2,1) := Integer(Sin(Rotation_Angle));
      Rotation_Matrix(2,2) := Integer(Cos(Rotation_Angle));
      Rotation_Matrix(3,3) := 1;
   else
      raise Rotation_Axis_Error;
   end if;
   return Rotation_Matrix;
end Get_Rotation_Matrix;

-- Skapar en multiplikator som behÃ¶vs fÃ¶r att rotera en vektor med rotationsmatrisen
function "*"(M : Int_Matrix; V : Int_Vector) return Int_Vector is
   V_Result : Int_Vector;
begin
   V_Result(1) := M(1,1)*V(1) + M(1,2)*V(2) + M(1,3)*V(3);
   V_Result(2) := M(2,1)*V(1) + M(2,2)*V(2) + M(2,3)*V(3);
   V_Result(3) := M(3,1)*V(1) + M(3,2)*V(2) + M(3,3)*V(3);
   return V_Result;
end "*";

-- Huvudfunktionen som roterar en vektor med en viss vinkel och kring en viss axel. Riktningen pÃ¥ rotationen sker efter hÃ¶gerhandsregeln
-- Fixar enbart 90, 180, 270 ... pga Integer istÃ¤llet fÃ¶r Float
function Rotate_Single_Point_Around_Axis(V_In : Int_Vector;
                                         Rotation_Angle : Float;
                                         Rotation_Axis : Character)
                                        return Int_Vector is
   Rotation_Matrix : Int_Matrix;
begin
   Rotation_Matrix := Get_Rotation_Matrix(Rotation_Angle, Rotation_Axis); -- Skapar rotationsmatrisen
   return Rotation_Matrix * V_In; -- Roterar vektorn med rotationsmatrisen
end Rotate_Single_Point_Around_Axis;

-- procedur som kallar pÃ¥ ovanstÃ¥ende funktion
procedure Rotate_Single_Point_Around_Axis(V : in out Int_Vector;
                                         Rotation_Angle : in Float;
                                         Rotation_Axis : in Character) is
begin
   V := Rotate_Single_Point_Around_Axis(V, Rotation_Angle, Rotation_Axis);
end Rotate_Single_Point_Around_Axis;

-- inte omplaceras
function Move_Single_Point(V : Int_Vector;
                            X, Y, Z : Integer) return Int_Vector is
begin
   return (V(1) + X, V(2) + Y, V(3) + Z);
end Move_Single_Point;

procedure Move_Single_Point(V : in out Int_Vector;
                            X, Y, Z : in Integer) is
begin
   V := Move_Single_Point(V, X, Y, Z);
end Move_Single_Point;

end Rotation_Package;


